function sim = mncp_fixed_point_pgs(sim)
% The difference with mncp_fixed_point is that this loop changes to contact point
% wise. By converge on the first contact point, and then next, until Nth
%factor = sim.solver.Rvalue;
%factor = 0.5;  % very good
factor = 0.4;
%factor = 0.7;
M = sim.dynamics.M;
Gn = sim.dynamics.Gn;
Gf = sim.dynamics.Gf;
h = sim.dynamics.h;
U = sim.dynamics.U;
FX = sim.dynamics.Forces;
MinvPext = M \ (FX*h);
PSI = sim.contacts.gap;
NU = sim.dynamics.Vel;
nc = length(PSI);
 
G = zeros(length(NU), nc*3);

Gt = zeros(length(NU), nc);
Go = zeros(length(NU), nc);
for i = 1 : nc 
    G(:, 3*i-2:3*i) = [Gn(:, i) Gf(:, 2*i-1:2*i)];
    Gt(:, i) = Gf(:, 2*i-1);
    Go(:, i) = Gf(:, 2*i);
end
Pext = FX * h;

% NCPdata = struct();
% NCPdata.M = M;
% NCPdata.Gn = Gn;
% NCPdata.Gf = Gf;
% NCPdata.P_ext = Pext;
% NCPdata.h = h;
% NCPdata.psi_n = PSI;
% NCPdata.U = U;
% NCPdata.G_t = Gt;
% NCPdata.G_o = Go;
% NCPdata.NUell = NU;

% mkdir('NCPdata');
% save ('NCPdata/NCPdata.mat', 'NCPdata');
% save('NCPdata/M.mat', 'M');
% save('NCPdata/G_n.mat', 'Gn');
% save('NCPdata/G_f.mat', 'Gf');
% save('NCPdata/P_ext.mat', 'Pext');
% save('NCPdata/h.mat', 'h');
% save ('NCPdata/psi_n.mat', 'PSI');
% save ('NCPdata/U.mat', 'U');
% save('NCPdata/G_t.mat', 'Gt');
% save('NCPdata/G_o.mat', 'Go');
% save('NCPdata/NUell.mat', 'NU');
% pause
nd = 2;
A = zeros(4*nc, 4*nc);
% A = [ Gn'*MinvGn   Gn'*MinvGf  zeros(nc)
%       Gf'*MinvGn   Gf'*MinvGf  E
%       U            -E'         zeros(nc)];
if exist('warm_start')
    warm_start = sim.warmstart;
else
    warm_start = 'quad_program';
end
if isfield(sim, 'constraints')
    % bilateral constraints
    Gb = sim.Ndynamics.Gb;
    Bounds = sim.constraints.bounds';
    Violation = sim.constraints.violation;
    nj = length(Violation);
    MinvGb     = M \ Gb;
    [~, pb_size] = size(Gb);
    pb = zeros(pb_size, 1);
    rb = 0.3 / eigs(Gb' * MinvGb, 1);
end
% bilateral constraints 
MinvGn     = M \ Gn;
MinvGf     = M \ Gf;
% should be less than 2/eig

 rn = factor / eigs(Gn' * MinvGn, 1);
 rf = 3 * factor / eigs(Gf' * MinvGf, 1);
 temp = 1/2 * (eigs(Gn' * MinvGn, 1) + eigs(Gf' * MinvGf, 1));
 rs = 3 * factor / temp;
maxIter1   = 500;      % the maximum number of iteration steps
 
     
 
toler = 1e-4;
%converge = zeros(maxIter1, 1);
% initial values for pf  
% pn = zeros(nc, 1);
% pf = zeros(2*nc, 1);
% s = zeros(nc, 1);

pn = rand(nc, 1);
%pf = rand(2*nc, 1);
%s = rand(nc, 1);

s = zeros(nc, 1);
s_ellp1 = zeros(nc, 1);
pf = zeros(2*nc, 1);
pf_ellp1 = zeros(2*nc, 1);
s_ellp1 = zeros(nc, 1);                                

solution  = struct();
solution.total_error = zeros(maxIter1, 1);
solution.normal_error = zeros(maxIter1, 1); 
solution.friction_error = zeros(maxIter1, 1) ;
solution.stick_or_slide = zeros(maxIter1, 1);
solution.z = zeros(4*nc, maxIter1);
solution.iterations = 0;
solution.direction_error = zeros(maxIter1, 1);
solution.copositive_normal_error = zeros(maxIter1, 1);
solution.copositive_friction_error = zeros(maxIter1, 1);
solution.normal_neg_error = zeros(maxIter1, 1);
solution.fric_neg_error = zeros(maxIter1, 1);

% norm_err = zeros(nc, maxIter1);
% fric_err = zeros(nc, maxIter1);
% to save the number of contacts: sliding; sticking; penetrate or detach.
% stick_or_slide = zeros(nc, maxIter1);   % slide = 1
% pene_or_deta   = zeros(nc, maxIter1);   % pene  = 1 
Anorm       = Gn'*MinvGn;
bnorm       = Gn'*(NU + MinvPext) + PSI/h - Gn'*MinvGf*pf;
switch warm_start
    case 'Lemke'
        pn_ellp1 = lemke(Anorm, bnorm, pn);
    case 'quad_program'
        %opts  = optimset('Algorithm', 'active-set', 'Display', 'off');
        opts  = optimset('Algorithm', 'interior-point-convex', 'Display', 'off');
        cons_A = [-1*eye(length(bnorm));  -Anorm];
        cons_b = [zeros(length(bnorm), 1);  bnorm];
        Anorm = (Anorm + Anorm');
        pn_ellp1 = quadprog(Anorm, bnorm, cons_A, cons_b, [], [], [], [], [], opts);
    otherwise
        pn_ellp1 = pn;  % when there is no warm start
end
pn = pn_ellp1;
%total_err = 0;
%converge = zeros(maxIter1, 1);
for iter1 = 1 : maxIter1  % one iteration is a loop over all the contacts
    %sumErr = 0;
    for CT = 1 : nc       % loop over all the contacts, each time solve one contact: both normal and friction
        %%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % normal part
%         DeMn = Gn' * MinvGn;
%         rn = 1 / (DeMn(nc, nc));
%         DeMf = Gf' * MinvGf;
%         rf = 0.5 / (sum(abs(DeMf(2*nc-1, :)))  + sum(abs(DeMf(2*nc, :))));
%         rs = 0.5 * (rn + rf);
        pn_ellp1(CT, 1)  = update_normal(pn(CT, 1),  rn, PSI(CT, 1), h, Gn(:, CT), NU);
        
        %%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % friction part
        pf_ellp1(2*CT-1:2*CT, 1)  = update_fric(pf(2*CT-1:2*CT, 1), rf, Gf(:, 2*CT-1: 2*CT), NU, pn_ellp1(CT, 1), U(CT, CT));

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [s_ellp1(CT, 1)] = update_slide(s(CT,1), pn(CT, 1), pf(2*CT-1:2*CT, 1), U(CT, CT), rs);
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        delta_pn = pn_ellp1 - pn;
        delta_pf = pf_ellp1 - pf;
        [NU_ellp1] = update_vel(NU, MinvGn, delta_pn, MinvGf, delta_pf, MinvPext); 
    end
        NU = NU_ellp1;
        pf = pf_ellp1;
        pn = pn_ellp1;
        s = s_ellp1;
z  =[ pn; pf; s];
b = setb(Gn, Gf, NU, MinvPext, PSI, h, nc);
% get the magnitude of the frictional force 
% pfMag = zeros(nc, 1);
% for CT = 1 : nc
%       pfMag(CT, 1)= norm(pf(2*CT-1:2*CT, 1));        
% end
solution =  updateSolutionData(solution, iter1, A, z, b, s, U, pn, PSI, pf, nd, Gf, NU_ellp1);
solution.name = 'mncp_fixed_point';

if solution.total_error(iter1) <  toler
    break;
end
end
 
sim.solution = solution;
end

function NU_ellp1 = update_vel(NU, MinvGn, pn, MinvGf, pf, MinvPext)
NU_ellp1 = NU + MinvGn * pn + MinvGf * pf + MinvPext;
end

function pn_ellp1 = update_normal(pn, rn, PSI, h, Gn, NU_ellp1)
% scalar operations below
% Pn_ellp1     = prox(Pn - rn (PSI/h + Gn'*NU_ellp1))
rhon = PSI/h + Gn'* NU_ellp1;
pn_temp = pn - rn*rhon;
% The normal force can not be negative, project onto the non negative space
pn_ellp1 = pn_temp;
% if pn_ellp1 <= 0   % detach
%     flag = -1;
%     pn_ellp1 = 0;
% else
%     flag = 1;      % >0  contact
% end
% err = PSI * pn_ellp1;
end

function pf_ellp1 = update_fric(pf, rf, Gf, NU_ellp1, pn_ellp1,U)
% Pf_ellp1 = prox(Pf - rf * (Gf * NU_ellp1 + s))
rhof = Gf' * NU_ellp1;
pf_temp = pf - rf * rhof;
%rel_vel = Gf' * NU_ellp1;
%rel_vel_dir = rel_vel / norm(rel_vel);
%pf_ellp1 = zeros(2, 1);
% The frictional force should be projected inside or onto the cone
% sliding case , error is frictional force - mu * normal force
if pn_ellp1 > 1e-6  && (norm(pf_temp) > U * pn_ellp1)
        pf_temp = U*pn_ellp1 * (pf_temp / (norm(pf_temp)));    
end
 
pf_ellp1 = pf_temp;
end


function [s_ellp1] = update_slide(s, pn, pf, U, rs)
     rhos = U*pn - norm(pf);
     sTemp = s - rs * rhos;
     if sTemp < 0 
         sTemp = 0;
     end
     s_ellp1 = sTemp;
end

function b = setb(Gn, Gf, NU, MinvPext, PSI, h, nc)
b = [ Gn'*(NU + MinvPext) + PSI/h;    % FX*h could be replaced if we stored Pext instead of Fext
      Gf'*(NU + MinvPext);
      zeros(nc,1) ];    
end
    % right now in the code, we have only spherical joint, which the size
    % should be 3
    % we don't loop joint by joint, since provided the big jacobian matrix
    % as a whole, here will solve all the Pb at the same time. Besides, the
    % Pb to solve is an equation, no iteration is needed.
    
    % Idety = ones(pb_size, 1);
    % how does this come from? we have pb in the form of equations,
    % rather than complementarity form, how does the prox come then ????
    % The prox here actually is we didn't project onto the positive plane,
    % but the complementarity condition doesn't exist in the physical
    % model, don't do projection onto the positive plane here. 
    % pb = min(Bounds(:, 2) * Idety, max(Bounds(:, 1) * Idety, pb - rb * (Gb' * NU_ellp1)));    